//
// File: xzhgeqz.cpp
//
// MATLAB Coder version            : 5.1
// C/C++ source code generated on  : 22-Feb-2021 03:11:30
//

// Include Files
#include "xzhgeqz.h"
#include "FLAE_data.h"
#include "FLAE_rtwutil.h"
#include "rt_nonfinite.h"
#include "sqrt.h"
#include "xzlanhs.h"
#include "xzlartg.h"
#include <cmath>

// Function Definitions
//
// Arguments    : creal_T A[16]
//                int ilo
//                int ihi
//                creal_T Z[16]
//                int *info
//                creal_T alpha1[4]
//                creal_T beta1[4]
// Return Type  : void
//
namespace coder
{
  namespace internal
  {
    namespace reflapack
    {
      void xzhgeqz(creal_T A[16], int ilo, int ihi, creal_T Z[16], int *info,
                   creal_T alpha1[4], creal_T beta1[4])
      {
        creal_T shift;
        double ad22_im;
        double ad22_re;
        double anorm;
        double ascale;
        double b_atol;
        double c;
        double ctemp_im;
        double ctemp_re;
        double eshift_im;
        double eshift_re;
        double g2;
        double temp2;
        int ilast;
        int j;
        int jp1;
        boolean_T failed;
        boolean_T guard1 = false;
        boolean_T guard2 = false;
        *info = 0;
        alpha1[0].re = 0.0;
        alpha1[0].im = 0.0;
        beta1[0].re = 1.0;
        beta1[0].im = 0.0;
        alpha1[1].re = 0.0;
        alpha1[1].im = 0.0;
        beta1[1].re = 1.0;
        beta1[1].im = 0.0;
        alpha1[2].re = 0.0;
        alpha1[2].im = 0.0;
        beta1[2].re = 1.0;
        beta1[2].im = 0.0;
        alpha1[3].re = 0.0;
        alpha1[3].im = 0.0;
        beta1[3].re = 1.0;
        beta1[3].im = 0.0;
        eshift_re = 0.0;
        eshift_im = 0.0;
        ctemp_re = 0.0;
        ctemp_im = 0.0;
        anorm = xzlanhs(A, ilo, ihi);
        temp2 = 2.2204460492503131E-16 * anorm;
        b_atol = 2.2250738585072014E-308;
        if (temp2 > 2.2250738585072014E-308) {
          b_atol = temp2;
        }

        temp2 = 2.2250738585072014E-308;
        if (anorm > 2.2250738585072014E-308) {
          temp2 = anorm;
        }

        ascale = 1.0 / temp2;
        failed = true;
        jp1 = ihi + 1;
        for (j = jp1; j < 5; j++) {
          alpha1[j - 1] = A[(j + ((j - 1) << 2)) - 1];
        }

        guard1 = false;
        guard2 = false;
        if (ihi >= ilo) {
          int ifirst;
          int iiter;
          int ilastm1;
          int istart;
          int jiter;
          boolean_T goto60;
          boolean_T goto70;
          boolean_T goto90;
          ifirst = ilo;
          istart = ilo;
          ilast = ihi - 1;
          ilastm1 = ihi - 2;
          iiter = 0;
          goto60 = false;
          goto70 = false;
          goto90 = false;
          jiter = 0;
          int exitg1;
          do {
            exitg1 = 0;
            if (jiter <= 30 * ((ihi - ilo) + 1) - 1) {
              boolean_T b_guard1 = false;
              boolean_T exitg2;
              boolean_T guard3 = false;
              b_guard1 = false;
              if (ilast + 1 == ilo) {
                goto60 = true;
                b_guard1 = true;
              } else {
                jp1 = ilast + (ilastm1 << 2);
                if (std::abs(A[jp1].re) + std::abs(A[jp1].im) <= b_atol) {
                  A[jp1].re = 0.0;
                  A[jp1].im = 0.0;
                  goto60 = true;
                  b_guard1 = true;
                } else {
                  j = ilastm1;
                  guard3 = false;
                  exitg2 = false;
                  while ((!exitg2) && (j + 1 >= ilo)) {
                    if (j + 1 == ilo) {
                      guard3 = true;
                      exitg2 = true;
                    } else {
                      jp1 = j + ((j - 1) << 2);
                      if (std::abs(A[jp1].re) + std::abs(A[jp1].im) <= b_atol) {
                        A[jp1].re = 0.0;
                        A[jp1].im = 0.0;
                        guard3 = true;
                        exitg2 = true;
                      } else {
                        j--;
                        guard3 = false;
                      }
                    }
                  }

                  if (guard3) {
                    ifirst = j + 1;
                    goto70 = true;
                  }

                  if (goto70) {
                    b_guard1 = true;
                  } else {
                    alpha1[0].re = rtNaN;
                    alpha1[0].im = 0.0;
                    beta1[0].re = rtNaN;
                    beta1[0].im = 0.0;
                    alpha1[1].re = rtNaN;
                    alpha1[1].im = 0.0;
                    beta1[1].re = rtNaN;
                    beta1[1].im = 0.0;
                    alpha1[2].re = rtNaN;
                    alpha1[2].im = 0.0;
                    beta1[2].re = rtNaN;
                    beta1[2].im = 0.0;
                    alpha1[3].re = rtNaN;
                    alpha1[3].im = 0.0;
                    beta1[3].re = rtNaN;
                    beta1[3].im = 0.0;
                    for (jp1 = 0; jp1 < 16; jp1++) {
                      Z[jp1].re = rtNaN;
                      Z[jp1].im = 0.0;
                    }

                    *info = 1;
                    exitg1 = 1;
                  }
                }
              }

              if (b_guard1) {
                if (goto60) {
                  goto60 = false;
                  alpha1[ilast] = A[ilast + (ilast << 2)];
                  ilast = ilastm1;
                  ilastm1--;
                  if (ilast + 1 < ilo) {
                    failed = false;
                    guard2 = true;
                    exitg1 = 1;
                  } else {
                    iiter = 0;
                    eshift_re = 0.0;
                    eshift_im = 0.0;
                    jiter++;
                  }
                } else {
                  if (goto70) {
                    double ascale_im;
                    double shift_re;
                    double tempr;
                    int ctemp_re_tmp;
                    int ctemp_re_tmp_tmp;
                    goto70 = false;
                    iiter++;
                    if (iiter - iiter / 10 * 10 != 0) {
                      double t1_im;
                      double t1_im_tmp;
                      double t1_re;
                      jp1 = ilastm1 + (ilastm1 << 2);
                      anorm = ascale * A[jp1].re;
                      temp2 = ascale * A[jp1].im;
                      jp1 = ilast + (ilast << 2);
                      tempr = ascale * A[jp1].re;
                      g2 = ascale * A[jp1].im;
                      if (temp2 == 0.0) {
                        shift.re = anorm / 0.5;
                        shift.im = 0.0;
                      } else if (anorm == 0.0) {
                        shift.re = 0.0;
                        shift.im = temp2 / 0.5;
                      } else {
                        shift.re = anorm / 0.5;
                        shift.im = temp2 / 0.5;
                      }

                      if (g2 == 0.0) {
                        ad22_re = tempr / 0.5;
                        ad22_im = 0.0;
                      } else if (tempr == 0.0) {
                        ad22_re = 0.0;
                        ad22_im = g2 / 0.5;
                      } else {
                        ad22_re = tempr / 0.5;
                        ad22_im = g2 / 0.5;
                      }

                      t1_re = 0.5 * (shift.re + ad22_re);
                      t1_im = 0.5 * (shift.im + ad22_im);
                      t1_im_tmp = t1_re * t1_im;
                      jp1 = ilastm1 + (ilast << 2);
                      anorm = ascale * A[jp1].re;
                      temp2 = ascale * A[jp1].im;
                      jp1 = ilast + (ilastm1 << 2);
                      tempr = ascale * A[jp1].re;
                      g2 = ascale * A[jp1].im;
                      if (temp2 == 0.0) {
                        c = anorm / 0.5;
                        ascale_im = 0.0;
                      } else if (anorm == 0.0) {
                        c = 0.0;
                        ascale_im = temp2 / 0.5;
                      } else {
                        c = anorm / 0.5;
                        ascale_im = temp2 / 0.5;
                      }

                      if (g2 == 0.0) {
                        temp2 = tempr / 0.5;
                        anorm = 0.0;
                      } else if (tempr == 0.0) {
                        temp2 = 0.0;
                        anorm = g2 / 0.5;
                      } else {
                        temp2 = tempr / 0.5;
                        anorm = g2 / 0.5;
                      }

                      shift_re = shift.re * ad22_re - shift.im * ad22_im;
                      tempr = shift.re * ad22_im + shift.im * ad22_re;
                      shift.re = ((t1_re * t1_re - t1_im * t1_im) + (c * temp2 -
                        ascale_im * anorm)) - shift_re;
                      shift.im = ((t1_im_tmp + t1_im_tmp) + (c * anorm +
                        ascale_im * temp2)) - tempr;
                      b_sqrt(&shift);
                      if ((t1_re - ad22_re) * shift.re + (t1_im - ad22_im) *
                          shift.im <= 0.0) {
                        shift.re += t1_re;
                        shift.im += t1_im;
                      } else {
                        shift.re = t1_re - shift.re;
                        shift.im = t1_im - shift.im;
                      }
                    } else {
                      jp1 = ilast + (ilastm1 << 2);
                      anorm = ascale * A[jp1].re;
                      temp2 = ascale * A[jp1].im;
                      if (temp2 == 0.0) {
                        c = anorm / 0.5;
                        ascale_im = 0.0;
                      } else if (anorm == 0.0) {
                        c = 0.0;
                        ascale_im = temp2 / 0.5;
                      } else {
                        c = anorm / 0.5;
                        ascale_im = temp2 / 0.5;
                      }

                      eshift_re += c;
                      eshift_im += ascale_im;
                      shift.re = eshift_re;
                      shift.im = eshift_im;
                    }

                    j = ilastm1;
                    jp1 = ilastm1 + 1;
                    exitg2 = false;
                    while ((!exitg2) && (j + 1 > ifirst)) {
                      istart = j + 1;
                      ctemp_re_tmp_tmp = j << 2;
                      ctemp_re_tmp = j + ctemp_re_tmp_tmp;
                      ctemp_re = ascale * A[ctemp_re_tmp].re - shift.re * 0.5;
                      ctemp_im = ascale * A[ctemp_re_tmp].im - shift.im * 0.5;
                      anorm = std::abs(ctemp_re) + std::abs(ctemp_im);
                      jp1 += ctemp_re_tmp_tmp;
                      temp2 = ascale * (std::abs(A[jp1].re) + std::abs(A[jp1].im));
                      tempr = anorm;
                      if (temp2 > anorm) {
                        tempr = temp2;
                      }

                      if ((tempr < 1.0) && (tempr != 0.0)) {
                        anorm /= tempr;
                        temp2 /= tempr;
                      }

                      jp1 = j + ((j - 1) << 2);
                      if ((std::abs(A[jp1].re) + std::abs(A[jp1].im)) * temp2 <=
                          anorm * b_atol) {
                        goto90 = true;
                        exitg2 = true;
                      } else {
                        jp1 = j;
                        j--;
                      }
                    }

                    if (!goto90) {
                      istart = ifirst;
                      ctemp_re_tmp = (ifirst + ((ifirst - 1) << 2)) - 1;
                      ctemp_re = ascale * A[ctemp_re_tmp].re - shift.re * 0.5;
                      ctemp_im = ascale * A[ctemp_re_tmp].im - shift.im * 0.5;
                    }

                    goto90 = false;
                    jp1 = istart + ((istart - 1) << 2);
                    ad22_re = ascale * A[jp1].re;
                    ad22_im = ascale * A[jp1].im;
                    ascale_im = std::abs(ctemp_re);
                    temp2 = std::abs(ctemp_im);
                    if (temp2 > ascale_im) {
                      ascale_im = temp2;
                    }

                    temp2 = std::abs(ad22_re);
                    anorm = std::abs(ad22_im);
                    if (anorm > temp2) {
                      temp2 = anorm;
                    }

                    anorm = ascale_im;
                    if (temp2 > ascale_im) {
                      anorm = temp2;
                    }

                    shift.re = ctemp_re;
                    shift.im = ctemp_im;
                    guard3 = false;
                    if (anorm >= 7.4428285367870146E+137) {
                      do {
                        shift.re *= 1.3435752215134178E-138;
                        shift.im *= 1.3435752215134178E-138;
                        ad22_re *= 1.3435752215134178E-138;
                        ad22_im *= 1.3435752215134178E-138;
                        anorm *= 1.3435752215134178E-138;
                      } while (!(anorm < 7.4428285367870146E+137));

                      guard3 = true;
                    } else if (anorm <= 1.3435752215134178E-138) {
                      if ((ad22_re == 0.0) && (ad22_im == 0.0)) {
                        c = 1.0;
                        shift.re = 0.0;
                        shift.im = 0.0;
                      } else {
                        do {
                          shift.re *= 7.4428285367870146E+137;
                          shift.im *= 7.4428285367870146E+137;
                          ad22_re *= 7.4428285367870146E+137;
                          ad22_im *= 7.4428285367870146E+137;
                          anorm *= 7.4428285367870146E+137;
                        } while (!(anorm > 1.3435752215134178E-138));

                        guard3 = true;
                      }
                    } else {
                      guard3 = true;
                    }

                    if (guard3) {
                      tempr = shift.re * shift.re + shift.im * shift.im;
                      g2 = ad22_re * ad22_re + ad22_im * ad22_im;
                      temp2 = g2;
                      if (1.0 > g2) {
                        temp2 = 1.0;
                      }

                      if (tempr <= temp2 * 2.0041683600089728E-292) {
                        if ((ctemp_re == 0.0) && (ctemp_im == 0.0)) {
                          c = 0.0;
                          g2 = rt_hypotd_snf(ad22_re, ad22_im);
                          shift.re = ad22_re / g2;
                          shift.im = -ad22_im / g2;
                        } else {
                          tempr = std::sqrt(g2);
                          c = rt_hypotd_snf(shift.re, shift.im) / tempr;
                          if (ascale_im > 1.0) {
                            g2 = rt_hypotd_snf(ctemp_re, ctemp_im);
                            shift.re = ctemp_re / g2;
                            shift.im = ctemp_im / g2;
                          } else {
                            anorm = 7.4428285367870146E+137 * ctemp_re;
                            temp2 = 7.4428285367870146E+137 * ctemp_im;
                            g2 = rt_hypotd_snf(anorm, temp2);
                            shift.re = anorm / g2;
                            shift.im = temp2 / g2;
                          }

                          ad22_re /= tempr;
                          ad22_im = -ad22_im / tempr;
                          temp2 = shift.re * ad22_re - shift.im * ad22_im;
                          anorm = shift.re * ad22_im + shift.im * ad22_re;
                          shift.re = temp2;
                          shift.im = anorm;
                        }
                      } else {
                        temp2 = std::sqrt(g2 / tempr + 1.0);
                        anorm = shift.re;
                        ascale_im = shift.im;
                        c = 1.0 / temp2;
                        g2 += tempr;
                        shift_re = temp2 * anorm / g2;
                        tempr = temp2 * ascale_im / g2;
                        shift.re = shift_re * ad22_re - tempr * -ad22_im;
                        shift.im = shift_re * -ad22_im + tempr * ad22_re;
                      }
                    }

                    j = istart;
                    jp1 = istart - 2;
                    while (j < ilast + 1) {
                      if (j > istart) {
                        ctemp_re_tmp_tmp = j + (jp1 << 2);
                        xzlartg(A[ctemp_re_tmp_tmp - 1], A[ctemp_re_tmp_tmp], &c,
                                &shift, &A[(j + (jp1 << 2)) - 1]);
                        A[ctemp_re_tmp_tmp].re = 0.0;
                        A[ctemp_re_tmp_tmp].im = 0.0;
                      }

                      for (jp1 = j; jp1 < 5; jp1++) {
                        ctemp_re_tmp = j + ((jp1 - 1) << 2);
                        ad22_re = c * A[ctemp_re_tmp - 1].re + (shift.re *
                          A[ctemp_re_tmp].re - shift.im * A[ctemp_re_tmp].im);
                        ad22_im = c * A[ctemp_re_tmp - 1].im + (shift.re *
                          A[ctemp_re_tmp].im + shift.im * A[ctemp_re_tmp].re);
                        anorm = A[ctemp_re_tmp - 1].im;
                        ascale_im = A[ctemp_re_tmp - 1].re;
                        A[ctemp_re_tmp].re = c * A[ctemp_re_tmp].re - (shift.re *
                          A[ctemp_re_tmp - 1].re + shift.im * A[ctemp_re_tmp - 1]
                          .im);
                        A[ctemp_re_tmp].im = c * A[ctemp_re_tmp].im - (shift.re *
                          anorm - shift.im * ascale_im);
                        A[ctemp_re_tmp - 1].re = ad22_re;
                        A[ctemp_re_tmp - 1].im = ad22_im;
                      }

                      shift.re = -shift.re;
                      shift.im = -shift.im;
                      ctemp_re_tmp_tmp = j;
                      if (ilast + 1 < j + 2) {
                        ctemp_re_tmp_tmp = ilast - 1;
                      }

                      for (int i = 1; i <= ctemp_re_tmp_tmp + 2; i++) {
                        ctemp_re_tmp = (i + ((j - 1) << 2)) - 1;
                        jp1 = (i + (j << 2)) - 1;
                        ad22_re = c * A[jp1].re + (shift.re * A[ctemp_re_tmp].re
                          - shift.im * A[ctemp_re_tmp].im);
                        ad22_im = c * A[jp1].im + (shift.re * A[ctemp_re_tmp].im
                          + shift.im * A[ctemp_re_tmp].re);
                        anorm = A[jp1].im;
                        ascale_im = A[jp1].re;
                        A[ctemp_re_tmp].re = c * A[ctemp_re_tmp].re - (shift.re *
                          A[jp1].re + shift.im * A[jp1].im);
                        A[ctemp_re_tmp].im = c * A[ctemp_re_tmp].im - (shift.re *
                          anorm - shift.im * ascale_im);
                        A[jp1].re = ad22_re;
                        A[jp1].im = ad22_im;
                      }

                      ctemp_re_tmp = (j - 1) << 2;
                      jp1 = j << 2;
                      ad22_re = c * Z[jp1].re + (shift.re * Z[ctemp_re_tmp].re -
                        shift.im * Z[ctemp_re_tmp].im);
                      ad22_im = c * Z[jp1].im + (shift.re * Z[ctemp_re_tmp].im +
                        shift.im * Z[ctemp_re_tmp].re);
                      anorm = Z[jp1].im;
                      ascale_im = Z[jp1].re;
                      Z[ctemp_re_tmp].re = c * Z[ctemp_re_tmp].re - (shift.re *
                        Z[jp1].re + shift.im * Z[jp1].im);
                      Z[ctemp_re_tmp].im = c * Z[ctemp_re_tmp].im - (shift.re *
                        anorm - shift.im * ascale_im);
                      Z[jp1].re = ad22_re;
                      Z[jp1].im = ad22_im;
                      ad22_re = c * Z[jp1 + 1].re + (shift.re * Z[ctemp_re_tmp +
                        1].re - shift.im * Z[ctemp_re_tmp + 1].im);
                      ad22_im = c * Z[jp1 + 1].im + (shift.re * Z[ctemp_re_tmp +
                        1].im + shift.im * Z[ctemp_re_tmp + 1].re);
                      anorm = Z[jp1 + 1].im;
                      ascale_im = Z[jp1 + 1].re;
                      Z[ctemp_re_tmp + 1].re = c * Z[ctemp_re_tmp + 1].re -
                        (shift.re * Z[jp1 + 1].re + shift.im * Z[jp1 + 1].im);
                      Z[ctemp_re_tmp + 1].im = c * Z[ctemp_re_tmp + 1].im -
                        (shift.re * anorm - shift.im * ascale_im);
                      Z[jp1 + 1].re = ad22_re;
                      Z[jp1 + 1].im = ad22_im;
                      ad22_re = c * Z[jp1 + 2].re + (shift.re * Z[ctemp_re_tmp +
                        2].re - shift.im * Z[ctemp_re_tmp + 2].im);
                      ad22_im = c * Z[jp1 + 2].im + (shift.re * Z[ctemp_re_tmp +
                        2].im + shift.im * Z[ctemp_re_tmp + 2].re);
                      anorm = Z[jp1 + 2].im;
                      ascale_im = Z[jp1 + 2].re;
                      Z[ctemp_re_tmp + 2].re = c * Z[ctemp_re_tmp + 2].re -
                        (shift.re * Z[jp1 + 2].re + shift.im * Z[jp1 + 2].im);
                      Z[ctemp_re_tmp + 2].im = c * Z[ctemp_re_tmp + 2].im -
                        (shift.re * anorm - shift.im * ascale_im);
                      Z[jp1 + 2].re = ad22_re;
                      Z[jp1 + 2].im = ad22_im;
                      ad22_re = c * Z[jp1 + 3].re + (shift.re * Z[ctemp_re_tmp +
                        3].re - shift.im * Z[ctemp_re_tmp + 3].im);
                      ad22_im = c * Z[jp1 + 3].im + (shift.re * Z[ctemp_re_tmp +
                        3].im + shift.im * Z[ctemp_re_tmp + 3].re);
                      anorm = Z[jp1 + 3].im;
                      ascale_im = Z[jp1 + 3].re;
                      Z[ctemp_re_tmp + 3].re = c * Z[ctemp_re_tmp + 3].re -
                        (shift.re * Z[jp1 + 3].re + shift.im * Z[jp1 + 3].im);
                      Z[ctemp_re_tmp + 3].im = c * Z[ctemp_re_tmp + 3].im -
                        (shift.re * anorm - shift.im * ascale_im);
                      Z[jp1 + 3].re = ad22_re;
                      Z[jp1 + 3].im = ad22_im;
                      jp1 = j - 1;
                      j++;
                    }
                  }

                  jiter++;
                }
              }
            } else {
              guard2 = true;
              exitg1 = 1;
            }
          } while (exitg1 == 0);
        } else {
          guard1 = true;
        }

        if (guard2) {
          if (failed) {
            *info = ilast + 1;
            for (jp1 = 0; jp1 <= ilast; jp1++) {
              alpha1[jp1].re = rtNaN;
              alpha1[jp1].im = 0.0;
              beta1[jp1].re = rtNaN;
              beta1[jp1].im = 0.0;
            }

            for (jp1 = 0; jp1 < 16; jp1++) {
              Z[jp1].re = rtNaN;
              Z[jp1].im = 0.0;
            }
          } else {
            guard1 = true;
          }
        }

        if (guard1) {
          for (j = 0; j <= ilo - 2; j++) {
            alpha1[j] = A[j + (j << 2)];
          }
        }
      }
    }
  }
}

//
// File trailer for xzhgeqz.cpp
//
// [EOF]
//
